#include "smooth.h"

#include <iostream>
#include <vector>
#include <cmath>
#include <limits>

// 计算点到直线的垂直距离
float perpendicularDistance(const Point& pt, const Point& lineStart, const Point& lineEnd) {
    // 计算线段长度平方
    const float segmentLengthSq =
        (lineEnd.x - lineStart.x) * (lineEnd.x - lineStart.x) + (lineEnd.y - lineStart.y) * (lineEnd.y - lineStart.y);

    // 处理零长度线段（起点终点重合）
    if (segmentLengthSq < std::numeric_limits<float>::epsilon()) {
        return std::hypot(pt.x - lineStart.x, pt.y - lineStart.y);
    }

    // 计算投影参数t
    const float t =
        ((pt.x - lineStart.x) * (lineEnd.x - lineStart.x) + (pt.y - lineStart.y) * (lineEnd.y - lineStart.y)) /
        segmentLengthSq;

    // 限制t在[0,1]范围内
    const float clampedT = std::max((float) 0.0, std::min((float) 1.0, t));

    // 计算投影点
    const Point projection{lineStart.x + clampedT * (lineEnd.x - lineStart.x),
                           lineStart.y + clampedT * (lineEnd.y - lineStart.y)};

    // 返回点到投影点的距离
    return std::hypot(pt.x - projection.x, pt.y - projection.y);
}

// 递归实现Douglas-Peucker算法
void simplifySegment(const std::vector<Point>& points, size_t startIdx, size_t endIdx, float epsilon,
                     std::vector<bool>& keepFlags) {
    // 基线情况：如果线段没有中间点
    if (endIdx <= startIdx + 1)
        return;

    // 找到离当前线段最远的点
    float        maxDistance = -1.0;
    size_t       maxIndex    = startIdx;
    const Point& startPoint  = points[startIdx];
    const Point& endPoint    = points[endIdx];

    for (size_t i = startIdx + 1; i < endIdx; ++i) {
        const float d = perpendicularDistance(points[i], startPoint, endPoint);
        if (d > maxDistance) {
            maxDistance = d;
            maxIndex    = i;
        }
    }

    // 如果最大距离超过阈值
    if (maxDistance > epsilon) {
        // 标记保留点
        keepFlags[maxIndex] = true;

        // 递归处理左右两段
        simplifySegment(points, startIdx, maxIndex, epsilon, keepFlags);
        simplifySegment(points, maxIndex, endIdx, epsilon, keepFlags);
    }
}

// 主简化函数
std::vector<Point> douglasPeucker(const std::vector<Point>& points, float epsilon) {
    if (points.size() < 3)
        return points;  // 无需简化

    std::vector<bool> keepFlags(points.size(), false);
    keepFlags.front() = keepFlags.back() = true;  // 始终保留首尾点

    // 开始递归处理
    simplifySegment(points, 0, points.size() - 1, epsilon, keepFlags);

    // 收集保留的点
    std::vector<Point> result;
    for (size_t i = 0; i < points.size(); ++i) {
        if (keepFlags[i]) {
            result.push_back(points[i]);
        }
    }
    return result;
}

std::vector<Point> computeDirections(const std::vector<Point>& path) {
    std::vector<Point> directions;
    const int          n = path.size();
    if (n < 2)
        return directions;

    for (int i = 0; i < n; ++i) {
        Point dir;
        if (i == 0) {  // 起始点方向
            dir = path[1] - path[0];
        } else if (i == n - 1) {  // 终点方向
            dir = path[n - 1] - path[n - 2];
        } else {  // 中间点方向：前后点平均方向
            dir = (path[i + 1] - path[i - 1]) * 0.5;
        }
        directions.push_back(dir);
    }

    return directions;
}

// 生成三阶贝塞尔控制点（k为曲率系数）
void generateControlPoints(const std::vector<Point>& path, const std::vector<Point>& directions,
                           std::vector<Point>& outCtrl, std::vector<Point>& inCtrl, float curvature) {
    const int n = path.size();
    outCtrl.clear();
    inCtrl.clear();

    for (int i = 0; i < path.size(); ++i) {
        // 出控制点：沿当前方向延伸
        Point cOut = path[i] + directions[i] * curvature;
        // 入控制点：沿反方向延伸（用于前一段曲线）
        Point cIn = path[i] - directions[i] * curvature;

        outCtrl.push_back(cOut);
        inCtrl.push_back(cIn);
    }
}

// 三阶贝塞尔曲线生成器
std::vector<Point> cubicBezier(const Point& p0, const Point& p1, const Point& p2, const Point& p3, int segments) {
    std::vector<Point> curve;
    for (int i = 0; i <= segments; ++i) {
        float t = static_cast<float>(i) / segments;
        float u = 1.0 - t;

        // 三阶贝塞尔公式
        Point pt = {u * u * u * p0.x + 3 * u * u * t * p1.x + 3 * u * t * t * p2.x + t * t * t * p3.x,
                    u * u * u * p0.y + 3 * u * u * t * p1.y + 3 * u * t * t * p2.y + t * t * t * p3.y};
        curve.push_back(pt);
    }
    return curve;
}

// 路径平滑主函数
std::vector<Point> smoothPath(const std::vector<Point>& path, float curvature, int segments) {
    std::vector<Point> smooth;
    const int          n = path.size();
    if (n < 2)
        return path;

    // 步骤1：计算方向向量
    auto directions = computeDirections(path);

    // 步骤2：生成控制点
    std::vector<Point> outCtrl, inCtrl;
    generateControlPoints(path, directions, outCtrl, inCtrl, curvature);

    // 步骤3：分段生成贝塞尔曲线
    for (int i = 0; i < n - 1; ++i) {
        auto curve = cubicBezier(path[i],        // 起点
                                 outCtrl[i],     // 出控制点
                                 inCtrl[i + 1],  // 入控制点（下一段的）
                                 path[i + 1],    // 终点
                                 segments);
        smooth.insert(smooth.end(), curve.begin(), curve.end());
    }

    return smooth;
}